#include <iostream>
#include <string.h>
#include <vector>
#include <stdexcept>
#include <math.h>
#include <cmath>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>

using namespace std;
using namespace Eigen;

class stewart
{
public:
    Eigen::VectorXd _initPose;
    Eigen::VectorXd _konwn_length;//正运动学已知量

    Eigen::Vector3d _pDown[6];
    Eigen::Vector3d _pUp[6];
    double _DownR;
    double _UpR;
    double _down_alpha;
    double _up_beta;
    
    // double lAxis[6];
    Eigen::VectorXd _inv_result;
public:
    stewart(double UpR,double DownR,double UpAlpha, double DownBeta);
    ~stewart();
    bool up_down_P_calcu(Eigen::Vector3d (& pDown)[6],  Eigen::Vector3d (& pUp)[6],double down_R,double up_R,double down_alpha,double up_beta);
    Eigen::VectorXd  inverse_fun2(Eigen::VectorXd pose,Eigen::VectorXd length, Eigen::Vector3d pDown[6],  Eigen::Vector3d pUp[6]);
    Eigen::Matrix<double,6,6>  jaco_calcu2(    Eigen::Vector3d _pDown[6],  Eigen::Vector3d _pUp[6],Eigen::VectorXd _pPose);
};


